﻿#include "algo_projection.h"
#include "algo_ode_solver.h"
#include "math/nonlinear_math.h"

#define SIGN(a) ((a) >= 0 ? 1 : -1)
#define ITERATION 15

#include "dbg.h"

double CAGD::Projection::Near(Handle(GeomCurve) curve, BPnt P, double &t, BPnt &Q, double tol)
{
    // 对每个有效节点区间等分寻找最近点
    int times = 0;
    double u0 = curve->FirstParameter();
    double u1 = curve->LastParameter();

    // 最近点投影不稳定，因此需要细分足够多次
    double near = P.Distance(curve->Value((u0 + u1) / 2));
    int N = (u1 - u0) * 100;
    for (int i = 0; i < N; i++)
    {
        double u = u0 + (u1 - u0) / (N - 1) * i;
        auto Q = curve->Value(u);
        double d = P.Distance(Q);
        if (d < near)
        {
            near = d;
            t = u;
        }
    }

    // 应用牛顿迭代法
    double dp = tol * 10;
    while (near > tol && dp > tol && times++ < ITERATION)
    {
        BVec V1, V2;
        curve->D2(t, Q, V1, V2);
        double u = t - (V1 * (Q - P)) / (V2 * (Q - P) + V1.SquareModulus());

        if (u >= u1)
            u = u1 - tol;
        if (u <= u0)
            u = u0 + tol;

        near = P.Distance(curve->Value(u));
        dp = V1.Modulus() * fabs(u - t);
        t = u;
    }
    Q = curve->Value(t);
    return near;
}

double CAGD::Projection::Ortho1(Handle(GeomCurve) curve, BPnt P, double &t, BPnt &Q, double tol)
{
    // 使用切线进行投影
    int times = 0;
    double u0 = curve->FirstParameter();
    double u1 = curve->LastParameter();
    t = (u0 + u1) / 2;

    double angle = tol * 10;
    while (angle > tol && times++ < ITERATION)
    {
        BPnt C0;
        BVec V1;
        curve->D1(t, C0, V1);

        // 距离足够小直接退出
        if (P.Distance(C0) < tol)
            break;

        // 计算 P 到切线上的投影点 Q，得到增量一阶方法
        BVec T = V1.Normalized();
        Q = C0 + T * ((P - C0) * T);
        double dt = V1 * (Q - C0) / V1.SquareModulus();

        angle = fabs(T * (P - C0).Normalized());
        t += dt;

        if (t >= u1)
            t = u1 - tol;
        if (t <= u0)
            t = u0 + tol;
    }
    Q = curve->Value(t);
    return angle;
}

double CAGD::Projection::Ortho2(Handle(GeomCurve) curve, BPnt P, double &t, BPnt &Q, double tol)
{
    // 使用曲率圆进行投影
    int times = 0;
    double u0 = curve->FirstParameter();
    double u1 = curve->LastParameter();
    t = (u0 + u1) / 2;

    double angle = tol * 10;
    while (angle > tol && times++ < ITERATION)
    {
        BPnt C0;
        BVec V1, V2;
        curve->D2(t, C0, V1, V2);

        // 距离足够小直接退出
        if (P.Distance(C0) < tol)
            break;

        BVec T = V1.Normalized();
        BVec B = (V1 % V2).Normalized();
        BVec N = B % T;

        // 考虑曲率为 0 的情况
        double kappa = (V1 % V2).Modulus() / pow(V1.Modulus(), 3);
        if (kappa < CAGD_ABS_ACC)
            return Near(curve, P, t, Q, tol);

        // 计算 P 到曲率圆上的投影点 Q，得到增量二阶方法
        BPnt O = C0 + N / kappa;
        BVec L = (B % (P - O)).Normalized() % B;
        Q = O + L / kappa;

        // 计算增量
        double dt = sqrt(2 * (V1 % (Q - C0)).Modulus() / kappa / pow(V1.Modulus(), 3));
        dt = fabs(dt) * SIGN(V1 * (Q - C0));

        angle = fabs(T * (P - C0).Normalized());
        t += dt;

        if (t >= u1)
            t = u1 - tol;
        if (t <= u0)
            t = u0 + tol;
    }
    Q = curve->Value(t);
    return angle;
}

double CAGD::Projection::Near(Handle(GeomSurface) surface, BPnt P, double &u, double &v, BPnt &Q, double tol)
{
    // 应用牛顿迭代法
    int times = 0;
    LinearSolver solver;

    double U1, U2, V1, V2;
    surface->Bounds(U1, U2, V1, V2);

    double dp = tol * 10;
    double near = P.Distance(surface->Value(u, v));
    while (near > tol && dp > tol && times++ < ITERATION)
    {
        BVec DU, DV, DUU, DUV, DVV;
        surface->D2(u, v, Q, DU, DV, DUU, DVV, DUV);

        auto dS = Q - P;
        double lSu = DU.Modulus();
        double lSv = DV.Modulus();

        Matrix J(2, 2);
        J(0, 0) = pow(lSu, 2) + DUU * dS;
        J(0, 1) = J(1, 0) = DU * DV + DUV * dS;
        J(1, 1) = pow(lSv, 2) + DVV * dS;

        Vector kappa(2);
        kappa[0] = -(DU * dS);
        kappa[1] = -(DV * dS);

        // 注意 J 不一定正定，因此使用标准解法
        Vector duv = solver.Solve(J, kappa, true);
        u += duv[0];
        v += duv[1];

        // 防止越界
        if (u <= U1)
            u = U1 + tol;
        if (u >= U2)
            u = U2 - tol;
        if (v <= V1)
            v = V1 + tol;
        if (v >= V2)
            v = V2 - tol;

        near = P.Distance(surface->Value(u, v));
        dp = fabs(lSu * duv[0] + lSv * duv[1]);
    }
    Q = surface->Value(u, v);
    return near;
}

double CAGD::Projection::Ortho1(Handle(GeomSurface) surface, BPnt P, double &u, double &v, BPnt &Q, double tol)
{
    // 切平面投影
    int times = 0;
    double angle = tol * 10;
    Vector uv(2);
    uv[0] = u;
    uv[1] = v;

    double U1, U2, V1, V2;
    surface->Bounds(U1, U2, V1, V2);

    LinearSolver solver;
    while (angle > tol && times++ < ITERATION)
    {
        // 计算各阶导向量
        BVec S10, S01;
        surface->D1(uv[0], uv[1], Q, S10, S01);

        // 投影方向，如果距离足够小就退出
        BVec dP = P - Q;
        if (dP.Modulus() < tol)
            break;

        Matrix I(2, 2);
        I(0, 0) = S10 * S10;
        I(0, 1) = I(1, 0) = S10 * S01;
        I(1, 1) = S01 * S01;

        // 沿着 Su Sv 方向进行分解（利用线性方程组）
        Vector b(2);
        b[0] = dP * S10;
        b[1] = dP * S01;

        // Cholesky 分解进行求解
        Vector lambda = solver.Cholesky(I, b, true);

        // 构造方程
        BVec C1 = S10 * lambda[0] + S01 * lambda[1];
        b[0] = C1 * S10;
        b[1] = C1 * S01;
        Vector duv = solver.Cholesky(I, b, true);
        uv = uv + duv;

        // 防止越界
        if (uv[0] <= U1)
            uv[0] = U1 + tol;
        if (uv[0] >= U2)
            uv[0] = U2 - tol;
        if (uv[1] <= V1)
            uv[1] = V1 + tol;
        if (uv[1] >= V2)
            uv[1] = V2 - tol;

        // 计算夹角
        BVec N = (S10 % S01).Normalized();
        angle = (N % dP.Normalized()).Modulus();
    }

    // 更新参数
    u = uv[0];
    v = uv[1];
    Q = surface->Value(u, v);

    return angle;
}

double CAGD::Projection::Ortho2(Handle(GeomSurface) surface, BPnt P, double &u, double &v, BPnt &Q, double tol)
{
    // 曲率投影
    int times = 0;
    double angle = tol * 10;
    Vector uv(2);
    uv[0] = u;
    uv[1] = v;

    double U1, U2, V1, V2;
    surface->Bounds(U1, U2, V1, V2);

    LinearSolver solver;
    while (angle > tol && times++ < ITERATION)
    {
        // 计算各阶导向量
        BVec S10, S01, S20, S02, S11;
        surface->D2(uv[0], uv[1], Q, S10, S01, S20, S02, S11);

        // 投影方向，如果距离足够小就退出
        BVec dP = P - Q;
        if (dP.Modulus() < tol)
            break;

        // 获得 I,II 基本形式的矩阵
        BVec N = (S10 % S01).Normalized();

        Matrix I(2, 2);
        I(0, 0) = S10 * S10;
        I(0, 1) = I(1, 0) = S10 * S01;
        I(1, 1) = S01 * S01;

        Matrix II(2, 2);
        II(0, 0) = N * S20;
        II(0, 1) = II(1, 0) = N * S11;
        II(1, 1) = N * S02;

        // 沿着 Su Sv 方向进行分解（利用线性方程组）
        Vector b(2);
        b[0] = dP * S10;
        b[1] = dP * S01;

        // Cholesky 分解进行求解
        Vector lambda = solver.Cholesky(I, b, true);

        // 计算法曲率
        double h = 0;
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++)
                h += II(i, j) * lambda[i] * lambda[j];

        double g = 0;
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++)
                g += I(i, j) * lambda[i] * lambda[j];

        // 法曲率，法曲率圆心，投影点 Q
        double kappa = h / g;

        // 如果遇到曲率为零，就使用最近点投影（稳定性不太好）
        if (fabs(kappa) < CAGD_ABS_ACC)
        {
            Near(surface, P, uv[0], uv[1], Q, tol);
            break;
        }

        double R = fabs(1 / kappa);
        BVec O = Q + N / kappa;
        BVec Q1 = O + (P - O).Normalized() * R;

        // 计算增量
        BVec C1 = S10 * lambda[0] + S01 * lambda[1];
        double dt = sqrt(2 * ((Q1 - Q) % C1).Modulus() * R / pow(C1.Modulus(), 3));
        dt = fabs(dt) * SIGN(C1 * (Q1 - Q));
        uv = uv + lambda * dt;

        // 防止越界
        if (uv[0] <= U1)
            uv[0] = U1 + tol;
        if (uv[0] >= U2)
            uv[0] = U2 - tol;
        if (uv[1] <= V1)
            uv[1] = V1 + tol;
        if (uv[1] >= V2)
            uv[1] = V2 - tol;

        // 计算夹角
        angle = (N % dP.Normalized()).Modulus();
    }

    // 更新参数
    u = uv[0];
    v = uv[1];
    Q = surface->Value(u, v);

    return angle;
}

double CAGD::Projection::Torus(Handle(GeomSurface) surface, BPnt P, double &u, double &v, BPnt &Q, double tol)
{
    // 曲率投影
    int times = 0;
    double angle = tol * 10;

    double U1, U2, V1, V2;
    surface->Bounds(U1, U2, V1, V2);

    LinearSolver solver;
    NewtonSolver fS;
    while (angle > tol && times++ < ITERATION)
    {
        // 计算各阶导向量
        BVec S10, S01, S20, S02, S11;
        surface->D2(u, v, Q, S10, S01, S20, S02, S11);

        // 距离足够小就退出
        if (P.Distance(Q) < tol)
            break;

        // 获得 I,II 基本形式的矩阵
        BVec N = (S10 % S01).Normalized();
        Matrix I(2, 2, {S10 * S10, S10 * S01, S01 * S10, S01 * S01});
        Matrix II(2, 2, {S20 * N, S11 * N, S11 * N, S02 * N});

        // 算出主曲率和主方向
        double H = (II(0, 0) * I(1, 1) - 2 * II(1, 0) * I(1, 0) + II(1, 1) * I(0, 0)) / I.Det() / 2;
        double K = II.Det() / I.Det();

        // 如果当前位置是脐点，就转而使用法曲率圆投影
        double Delta = H * H - K;
        if (Delta <= 0)
            return Ortho2(surface, P, u, v, Q, tol);

        // 主曲率
        double k1 = H + sqrt(Delta);
        double k2 = H - sqrt(Delta);

        // 主方向
        BVec e1 = S10 * (II(1, 0) - k1 * I(1, 0)) / (II(0, 0) - k1 * I(0, 0)) - S01;
        BVec e2 = S10 * (II(1, 0) - k2 * I(1, 0)) / (II(0, 0) - k2 * I(0, 0)) - S01;
        e1.Normalize();
        e2.Normalize();

        // 确保 |k1| <= |k2|
        if (fabs(k1) > fabs(k2))
        {
            double tk = k1;
            k1 = k2;
            k2 = tk;

            BVec ek = e1;
            e1 = e2;
            e2 = ek;
        }
        // 确保 k1 <= 0
        if (k1 > 0)
        {
            N = N * -1;
            k1 *= -1;
            k2 *= -1;
        }

        // 建立环面，得到投影点 Pr
        double R = -1 / k1 + 1 / k2;
        double r = fabs(1 / k2);
        BVec O = Q + N / k1;
        BVec OP = P - O;
        BVec OB = (OP - e2 * (OP * e2)).Normalized() * R;
        BVec Pr = O + OB + (OP - OB).Normalized() * r;

        // 计算初始增量
        BVec QPr = Pr - Q;

        // 沿着 Su Sv 方向进行分解（利用线性方程组），计算参数初始增量 duv
        Vector b = {QPr * S10, QPr * S01};
        Vector duv = solver.Cholesky(I, b, true);

        // 使用牛顿迭代法计算根
        FuncXX f = [=](Vector UV) -> Vector {
            BVec E = S10 * UV[0] + S01 * UV[1] +
                     (S20 * UV[0] * UV[0] + S11 * UV[0] * UV[1] * 2 + S02 * UV[1] * UV[1]) / 2 - QPr;
            return {E.X(), E.Y(), E.Z()};
        };

        // 获得增量
        fS(f, duv);
        u += duv[0];
        v += duv[1];

        // 防止越界
        if (u <= U1)
            u = U1 + tol;
        if (u >= U2)
            u = U2 - tol;
        if (v <= V1)
            v = V1 + tol;
        if (v >= V2)
            v = V2 - tol;

        // 夹角足够小
        angle = (N % ((P - Pr).Normalized())).Modulus();
    }
    Q = surface->Value(u, v);

    return angle;
}

void CAGD::Projection::March(Handle(GeomCurve) curve, Handle(GeomSurface) surface, double t0, double t1, int N,
                             double tol, std::vector<BPnt2d> &Para)
{
    // 首先计算一个单点的正交投影作为初始值，由于正交投影的稳定性，直接从曲面中心开始迭代投影
    BPnt P0 = curve->Value(t0);

    double U1, U2, V1_, V2_;
    surface->Bounds(U1, U2, V1_, V2_);

    double u = (U1 + U2) / 2;
    double v = (V1_ + V2_) / 2;

    BPnt Q;
    Ortho2(surface, P0, u, v, Q, tol);

    // 终止条件以及步长
    double k = (t1 - t0) / N;
    double t = t0;

    // 记录每一步的参数值
    std::vector<Vector> UV(N + 1, Vector(2));
    UV[0] = Vector({u, v});

    LinearSolver solver;

    // 迭代步进
    for (int i = 0; i < N; i++)
    {
        // 计算各阶导向量
        BVec S10, S01, S20, S02, S11, Np, N;
        surface->D2(u, v, Q, S10, S01, S20, S02, S11);
        Np = S10 % S01;
        N = curve->Value(t) - Q;

        // 构造系数矩阵 G
        Matrix G(2, 2);
        G(0, 0) = S10 * S10 + N * S20;
        G(0, 1) = G(1, 0) = S10 * S01 + N * S11;
        G(1, 1) = S01 * S01 + N * S02;
        // 注意 G 虽然对称，但不一定正定，不能使用 Cholesky 分解

        // 进行修正：N = q - p，比较 N 与 Np
        int fix = 0;
        double epsilon = (N.Normalized() % Np.Normalized()).Modulus();
        while (epsilon > tol && fix < ITERATION)
        {
            // 求解修正方程，同时替换参数
            Vector Nuv = {N * S10, N * S01};
            Vector duv = solver.Solve(G, Nuv, true);
            u += duv[0];
            v += duv[1];

            // 防止越界
            if (u <= U1)
                u = U1 + tol;
            if (u >= U2)
                u = U2 - tol;
            if (v <= V1_)
                v = V1_ + tol;
            if (v >= V2_)
                v = V2_ - tol;

            // 计算各阶导向量
            surface->D2(u, v, Q, S10, S01, S20, S02, S11);
            Np = S10 % S01;
            N = curve->Value(t) - Q;

            // 构造系数矩阵 G
            G(0, 0) = S10 * S10 + N * S20;
            G(0, 1) = G(1, 0) = S10 * S01 + N * S11;
            G(1, 1) = S01 * S01 + N * S02;

            epsilon = (N.Normalized() % Np.Normalized()).Modulus();
            fix++;
        }
        UV[i] = Vector({u, v});

        // 求解一阶偏导
        BVec V1, V2;
        curve->D2(t, Q, V1, V2);
        Vector b1 = {V1 * S10, V1 * S01};
        Vector uv1 = solver.Solve(G, b1, true);

        // 求解二阶偏导
        std::vector<std::vector<BPnt>> S;
        surface->DN(3, u, v, S);

        Matrix A1(2, 2);
        A1(0, 0) = 3 * (S[2][0] * S[1][0]) + (S[3][0] * N);
        A1(0, 1) = A1(1, 0) = 2 * (S[1][1] * S[1][0]) + (S[2][0] * S[0][1]) + (S[2][1] * N);
        A1(1, 1) = (S[0][2] * S[1][0]) + 2 * (S[1][1] * S[0][1]) + (S[1][2] * N);

        Matrix A2(2, 2);
        A2(0, 0) = (S[2][0] * S[0][1]) + 2 * (S[1][1] * S[1][0]) + (S[2][1] * N);
        A2(0, 1) = A2(1, 0) = 2 * (S[1][1] * S[0][1]) + (S[0][2] * S[1][0]) + (S[1][2] * N);
        A2(1, 1) = 3 * (S[0][2] * S[0][1]) + (S[0][3] * N);

        Vector b2(2);
        b2[0] = V2 * S[1][0];
        b2[0] += 2 * (V1 * (S[2][0] * uv1[0] + S[1][1] * uv1[1]));

        // 减去二阶项
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++)
                b2[0] -= A1(i, j) * uv1[i] * uv1[j];

        b2[1] = V2 * S[0][1];
        b2[1] += 2 * (V1 * (S[1][1] * uv1[0] + S[0][2] * uv1[1]));

        // 减去二阶项
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++)
                b2[1] -= A2(i, j) * uv1[i] * uv1[j];

        // 解得 2 阶偏导
        Vector uv2 = solver.Solve(G, b2, true);

        // 利用 2 阶泰勒展开推进
        u += uv1[0] * k + 0.5 * uv2[0] * k * k;
        v += uv1[1] * k + 0.5 * uv2[1] * k * k;

        // 防止越界
        if (u <= U1)
            u = U1 + tol;
        if (u >= U2)
            u = U2 - tol;
        if (v <= V1_)
            v = V1_ + tol;
        if (v >= V2_)
            v = V2_ - tol;

        // 记录这一步参数
        UV[i + 1] = Vector({u, v});
        t += k;
    }

    // 记录为参数域中的点
    for (int i = 0; i < UV.size(); i++)
        Para.push_back(BPnt2d(UV[i][0], UV[i][1]));
}

void CAGD::Projection::ODE(Handle(GeomCurve) curve, Handle(GeomSurface) surface, double t0, double t1, int N,
                           double tol, std::vector<BPnt2d> &Para)
{
    double U1, U2, V1_, V2_;
    surface->Bounds(U1, U2, V1_, V2_);

    double v1 = curve->FirstParameter();
    double v2 = curve->LastParameter();

    // 构造求解的函数
    FuncXXt F = [=](Vector x, double t) -> Vector {
        double h = CAGD_ABS_ACC;

        // 防止越界
        if (t <= v1)
            t = v1 + tol;
        if (t >= v2)
            t = v2 - tol;

        BPnt P;
        BVec V1;
        curve->D1(t, P, V1);

        // 防止越界
        if (x[0] <= U1)
            x[0] = U1 + tol;
        if (x[0] >= U2)
            x[0] = U2 - tol;
        if (x[1] <= V1_)
            x[1] = V1_ + tol;
        if (x[1] >= V2_)
            x[1] = V2_ - tol;

        // 计算各阶导向量
        BPnt Q;
        BVec g10, g01, g20, g02, g11;
        surface->D2(x[0], x[1], Q, g10, g01, g20, g02, g11);
        Vector y = {V1 * g10, V1 * g01};

        // 空间点到投影点的向量，获得投影张量矩阵
        BVec pq = Q - P;
        Matrix K(2, 2);
        K(0, 0) = g10 * g10 + pq * g20;
        K(0, 1) = g10 * g01 + pq * g11;
        K(1, 0) = g01 * g10 + pq * g11;
        K(1, 1) = g01 * g01 + pq * g02;
        // 此矩阵不一定正定，因此不使用 Cholesky

        // 获得微分方程右边的向量值
        LinearSolver solver;
        return solver.Solve(K, y, true);
    };

    // 首先计算一个单点的正交投影作为初始值，由于正交投影的稳定性，直接从曲面中心开始迭代投影
    BPnt P0 = curve->Value(t0);

    double u = (U1 + U2) / 2;
    double v = (V1_ + V2_) / 2;

    BPnt Q;
    Ortho2(surface, P0, u, v, Q, tol);

    // 终止条件以及步长
    double k = (t1 - t0) / N;

    // 使用经典 RK 方法
    ODESolver solver;
    ClassicalRK method(k);

    // 注意要多给一个
    TimeGrid G(N + 1, Vector(2));
    G[0] = Vector({u, v});
    solver(F, G, &method);

    // 记录为参数域中的点
    for (int i = 0; i < G.size(); i++)
    {
        auto &x = G[i];

        // 防止越界
        if (x[0] <= U1)
            x[0] = U1 + tol;
        if (x[0] >= U2)
            x[0] = U2 - tol;
        if (x[1] <= V1_)
            x[1] = V1_ + tol;
        if (x[1] >= V2_)
            x[1] = V2_ - tol;

        Para.push_back({x[0], x[1]});
    }
}
